﻿using System;

namespace VectorMath
{
    public class Pnt3F
    {
        public float x;
        public float y;
        public float z;

        public float SquaredLength { get { return x * x + y * y + z * z; } }

        public float Length { get { return (float)Math.Sqrt(SquaredLength); } }

        public Pnt3F(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        public void Add(Pnt3F p)
        {
            x += p.x;
            y += p.y;
            z += p.z;
        }

        public void Sub(Pnt3F p)
        {
            x -= p.x;
            y -= p.y;
            z -= p.z;
        }

        public void Times(float d)
        {
            x *= d;
            y *= d;
            z *= d;
        }

        /// <summary>
        /// this = this + (p * d) .
        /// </summary>
        public void ScaleAdd(Pnt3F p, float d)
        {
            x += p.x * d;
            y += p.y * d;
            z += p.z * d;
        }

        /// <summary>
        /// Set this point's value.
        /// </summary>
        public void Set(float x, float y, float z)
        {
            this.x = x;
            this.y = y;
            this.z = z;
        }

        /// <summary>
        /// Normalize this vector.
        /// </summary>
        public void Normalize()
        {
            float length = Length; //need to save this since it will change
            x /= length;
            y /= length;
            z /= length;
        }

        /// <summary>
        /// Returns this dot p.
        /// </summary>
        public float Dot(Pnt3F p)
        {
            return x * p.x + y * p.y + z * p.z;
        }

        /// <summary>
        /// Returns (y*p.z - z *p.y, z*p.x - x*p.z, x*p.y - y *p.x).
        /// </summary>
        public Pnt3F Cross(Pnt3F p)
        {
            return new Pnt3F(y * p.z - z * p.y, z * p.x - x * p.z, x * p.y - y * p.x);
        }

        /// <summary>
        /// Returns a new point equal to a - b.
        /// </summary>
        public static Pnt3F Difference(Pnt3F a, Pnt3F b)
        {
            return new Pnt3F(a.x - b.x, a.y - b.y, a.z - b.z);
        }

        public override bool Equals(object obj)
        {
            if (obj is Pnt3F)
                return Equals((Pnt3F)obj);
            else
                return base.Equals(obj);
        }

        public bool Equals(Pnt3F other)
        {
            return this.x == other.x && this.y == other.y && this.z == other.z;
        }

        public override int GetHashCode()
        {
            return (((x.GetHashCode() << 7) + y.GetHashCode()) << 7) + z.GetHashCode();
        }
    }
}
